/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Joystick;
import java.util.Timer;

/**
 *
 * @author Mark
 */
public class Utils
{

    public static int FRONT_TRIG_BUTTON_NUM = 1;

    public static int SIDE_TRIG_BUTTON_NUM = 2;

    public static int ALIGN_TRIG_BUTTON_NUM = 1;

    public static int WHEEL_TUNE_BUTTON_NUM = 8;

    public static int PID_TUNE_BUTTON_NUM = 12;

    public static double ANGULAR_SPEED = .2;

    public static double JOY_SENSITIVITY_FACTOR = .1;

    public static double TWIST_SENSITIVITY_FACTOR = .05;
    
    public static double PI = 3.14159265358979323846;

    private static int currAngSign = 0;

    public static int TARGET_TUNE_BUTTON_NUM = 2;

    public static int MAINTENANCE_MODE_BUTTON_NUM = 8;

    public static int LIFTER_LOCK_BUTTON_NUM_1 = 8;

    public static int LIFTER_LOCK_BUTTON_NUM_2 = 9;

    /**
     * Gets the sign of a value
     * @param value Value to get the sign of
     * @return +1 if positive or 0, -1 if negative
     */
    public static int signOf(double value)
    {
        if(value > 0)
        {
            return 1;
        }
        else
        {
            if(value < 0)
            {
                return -1;
            }
            else
            {
                return 1;
            }
        }
    }

    /**
     * Calculate the angle (in degrees) of the right triangle formed with legs
     * x and y
     * @param x horizontal leg (adjacent)
     * @param y vertical leg (opposite)
     * @return Angle (in degrees) formed
     */
    public static double calculateAng(double x, double y)
    {
        return (MathUtils.atan2(y, x)* 180)/Utils.PI;
    }

    /**
     * Calculate the hypotenuse of the right triangle formed by legs x and y
     * @param x Leg 1
     * @param y Leg 2
     * @return Hypotenuse length
     */
    public static double calculateMag(double x, double y)
    {
        return Math.sqrt(x*x + y*y);
    }

    /**
     * Checks to see if the value is outside of the [-1,1] bounds. If it is
     * just set it to the max value.
     * @param value Value to evaulate
     * @return Return same value if inside bounds. Return bound value if outside
     */
    public static double limit(double value)
    {
        if(Math.abs(value) <= 1)
        {
            return value;
        }
        else
        {
            if(value < -1)
            {
                return -1;
            }
            else
            {
                return 1;
            }
        }
    }

    public static int calculateAngularSign(double twist)
    {
        if(twist > Utils.JOY_SENSITIVITY_FACTOR && currAngSign != -1)
        {
            return -1;
        }
        else
        {
            if(twist < -Utils.JOY_SENSITIVITY_FACTOR && currAngSign != 1)
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }
    }

    public static double mod360(double angle)
    {
        while(angle < 0)
        {
            angle += 360;
        }

        return angle % 360;
    }


   /**
    * Format63f()
    *
    * The JVM in the cRIO does not support String.format()
    * It also doesn't support Math.round()
    * This method will format with specifier %6.3f
    * We use this to print speeds
    */
    public static String StringFormat63f(double dValue){

        String s = "";

        //long iValue = Math.abs(Math.round(dValue*1000.0));
        double dTemp = dValue*1000.0;
        long iValue = (long)Math.abs( dTemp > 0.0 ? dTemp + 0.5 : dTemp - 0.5 );

        long d1;        // integer digit(s)
        long d2;        // 1st decimal digit
        long d3;        // 2nd decimal digit
        long d4;        // 3rd decimal digit

        d1 = iValue / 1000;
        d2 = ( iValue - d1*1000 ) / 100;
        d3 = ( iValue - d1*1000 - d2*100 ) / 10;
        d4 = ( iValue - d1*1000 - d2*100 - d3*10 );


        if( dValue < 0.0 )
            s = s + "-";
        else
            s = s + " ";

        s = s + d1;

        s = s + ".";

        s = s + d2;
        s = s + d3;
        s = s + d4;

        return s;

    }


   /**
    * StringFormat61f()
    *
    * The JVM in the cRIO does not support String.format()
    * It also doesn't support Math.round()
    * This method will format with specifier %6.1f
    * We use this to print angles
    *
    */
    public static String StringFormat61f(double dValue){

        String s = "";

        //long iValue = Math.abs(Math.round(dValue*10.0));
        double dTemp = dValue*10.0;
        long iValue = (long)Math.abs( dTemp > 0.0 ? dTemp + 0.5 : dTemp - 0.5 );

        long d1;        // 1st integer digit(s)
        long d2;        // 2nd integer digit
        long d3;        // 3rd integer digit
        long d4;        // decimal digit

        d1 = ( iValue )/1000;
        d2 = ( iValue - d1*1000 )/100;
        d3 = ( iValue - d1*1000 - d2*100 )/10;
        d4 = ( iValue - d1*1000 - d2*100 - d3*10 )/1;

        if( iValue < 1000 )
            s = s + " ";
        if( iValue < 100 )
            s = s + " ";
        if( dValue < 0.0 )
            s = s + "-";
        else
            s = s + " ";

        if( iValue >= 1000 )
            s = s + d1 + d2 + d3;
        else if( iValue >= 100 )
            s = s + d2 + d3;
        else
            s = s + d3;

        s = s + ".";

        s = s + d4;

        return s;

    }


   /**
    * StringFormat4d()
    *
    * The JVM in the cRIO does not support String.format()
    * This method will format with specifier %4d
    * We use this to print encoder raw values
    *
    */
    public static String StringFormat6d(int iValue){

        String s = "";

        if(iValue>=0)
            s = s + " ";
        if(iValue<10000)
            s = s + " ";
        if(iValue<1000)
            s = s + " ";
        if(iValue<100)
            s = s + " ";
        if(iValue<10)
            s = s + " ";
        s = s + iValue;

        return s;

    }

    public static Timer getNewTimerObject()
    {
        return new Timer();
    }


    public static void clearUserMessages()
    {
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kMain6, 1, "                       ");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser2, 1, "                       ");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "                       ");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser4, 1, "                       ");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "                       ");
        DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser5, 1, "                       ");
        DriverStationLCD.getInstance().updateLCD();
    }
}
